// --------------------------------------------------------------------------------------------------------------------
// <copyright file="KukaLBR4Definitions.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

// This file provides shared controller register definitions between .cs and cpp files
// Symbol CSHARP is #defined for compiling as C#

#if !Kuka_LBR4_Definitions_defined
// Cannot use #pragma once in C# / cpp shared header file
#define Kuka_LBR4_Definitions_defined

/// <summary> Enumeration of working registers for Kuka LBR 4+ </summary>
#if CSHARP
namespace Microsoft.Robotics.Hardware.KukaLBR
{
    using System;
    using System.Diagnostics.CodeAnalysis;

    /// <summary>
    /// Hardware definitions for KUKA arm
    /// </summary>
    public struct KukaLBR4PlusCsharpDefinitionsNativeMethods
    {
        /// <summary>
        /// Number of cartesian pose dimensions
        /// </summary>
        public static uint CARTESIAN_DIM = 6;

        /// <summary>
        /// Number of elements in a Cartesian frame.  Represents the first 3
        /// rows of a homogenous transform in row-major order.  Last row is
        /// assumed to be [0 0 0 1].
        /// </summary>
        public static uint CARTESIAN_FRAME_LENGTH = 12;

        /// <summary>
        /// Number of arm joints
        /// </summary>
        public static uint NUM_JOINTS = 7;

        /// <summary>
        /// Maximum register size
        /// </summary>
        public static uint MAXIMUM_REGISTER_SIZE = CARTESIAN_DIM * NUM_JOINTS;

        /// <summary>
        /// Total number of all registers
        /// </summary>
        public static int KukaLBR4PlusTotalNumberOfRegisters =
        (int)KukaLBR4PlusWorkingRegisters.NumberOfWorkingRegisters + (int)KukaLBR4PlusFeedbackRegisters.NumberOfFeedbackRegisters;

        /// <summary>
        /// Working register sizes
        /// </summary>
        public static uint[] KukaLBR4PlusRegisterSizes =
#else
#pragma once
    const unsigned int KukaLBR4PlusRegisterSizes[] =
#endif
/// <summary> Size of each register in bytes. </summary>
{
    /////////////////// Working Registers //////////////////////
    1 * sizeof(double),                             // CommandedControllerStrategy
    NUM_JOINTS * sizeof(double),                    // CommandedJointPosition
    CARTESIAN_FRAME_LENGTH * sizeof(double),        // CommandedCartesianPosition
    NUM_JOINTS * sizeof(double),                    // CommandedJointTorque
    CARTESIAN_DIM * sizeof(double),                 // CommandedTCPForceTorque
    NUM_JOINTS * sizeof(double),                    // CommandedJointStiffness
    NUM_JOINTS * sizeof(double),                    // CommandedJointDamping
    CARTESIAN_DIM * sizeof(double),                 // CommandedCartesianStiffness
    CARTESIAN_DIM * sizeof(double),                 // CommandedCartesianDamping
    1 * sizeof(double),                             // Scalar1
    1 * sizeof(double),                             // Scalar2
    1 * sizeof(double),                             // Scalar3
    1 * sizeof(double),                             // Scalar4
    1 * sizeof(double),                             // Scalar5
    1 * sizeof(double),                             // Scalar6
    1 * sizeof(double),                             // Scalar7
    1 * sizeof(double),                             // Scalar8
    CARTESIAN_DIM * sizeof(double),                 // CartVec1
    CARTESIAN_DIM * sizeof(double),                 // CartVec2
    CARTESIAN_DIM * sizeof(double),                 // CartVec3
    NUM_JOINTS * sizeof(double),                    // JointVec1
    NUM_JOINTS * sizeof(double),                    // JointVec2
    NUM_JOINTS * sizeof(double),                    // JointVec3
    CARTESIAN_FRAME_LENGTH * sizeof(double),        // Frame1
    CARTESIAN_FRAME_LENGTH * sizeof(double),        // Frame2
    CARTESIAN_FRAME_LENGTH * sizeof(double),        // Frame3
    1 * sizeof(double),                             // Current controller state number

    //////////////////  Feedback Registers  ////////////////////
    1 * sizeof(double),                             // TimestampInMs
    1 * sizeof(double),                             // FRIState
    1 * sizeof(double),                             // UDPQuality
    1 * sizeof(double),                             // CycleTimeInMs
    1 * sizeof(double),                             // SafetyLimitInPercent
    1 * sizeof(double),                             // DrivePowerBitField
    1 * sizeof(double),                             // ControllerStrategy
    NUM_JOINTS * sizeof(double),                    // JointTemperature
    NUM_JOINTS * sizeof(double),                    // MeasuredJointPosition
    NUM_JOINTS * sizeof(double),                    // MeasuredJointVelocity 
    NUM_JOINTS * sizeof(double),                    // CommandedJointPosition
    NUM_JOINTS * sizeof(double),                    // CommandedJointOffset
    CARTESIAN_FRAME_LENGTH * sizeof(double),        // MeasuredCartesianPosition
    CARTESIAN_FRAME_LENGTH * sizeof(double),        // CommandedCartesianPosition
    CARTESIAN_FRAME_LENGTH * sizeof(double),        // CommandedCartesianOffset
    NUM_JOINTS * sizeof(double),                    // MeasuredJointTorque
    NUM_JOINTS * sizeof(double),                    // MeasuredExternalJointTorque
    CARTESIAN_DIM * sizeof(double),                 // EstimatedTCPForceTorque
    CARTESIAN_DIM * NUM_JOINTS * sizeof(double),    // Jacobian
    CARTESIAN_DIM * NUM_JOINTS * sizeof(double),    // MassMatrix
    NUM_JOINTS * sizeof(double),                    // GravityCompensation
    NUM_JOINTS * sizeof(double)                     // LastIssuedJointPositionCommand
};

#if CSHARP
        /// <summary>
        /// Working register enumeration
        /// </summary>
        public enum
#else
    enum KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT
#endif

 KukaLBR4PlusWorkingRegisters
        {
            /// <summary>
            /// Of dimension 1
            /// </summary>
            ControllerStrategyCommandRegister,

            /// <summary>
            /// Of dimension NUM_JOINTS
            /// </summary>
            JointPositionCommandRegister,

            /// <summary>
            /// Of dimension CARTESIAN_FRAME_LENGTH
            /// </summary>
            CartesianPositionCommandRegister,

            /// <summary>
            /// Of dimension NUM_JOINTS
            /// </summary>
            JointTorqueCommandRegister,

            /// <summary>
            /// Of dimension CARTESIAN_DIM
            /// </summary>
            TCPForceTorqueCommandRegister,

            /// <summary>
            /// Of dimension NUM_JOINTS
            /// </summary>
            JointStiffnessCommandRegister,

            /// <summary>
            /// Of dimension NUM_JOINTS
            /// </summary>
            JointDampingCommandRegister,

            /// <summary>
            /// Of dimension CARTESIAN_DIM
            /// </summary>
            CartesianStiffnessCommandRegister,

            /// <summary>
            /// Of dimension CARTESIAN_DIM
            /// </summary>
            CartesianDampingCommandRegister,

            /// <summary>
            /// Of dimension 1
            /// </summary>
            Scalar1Register,

            /// <summary>
            /// Of dimension 1
            /// </summary>
            Scalar2Register,

            /// <summary>
            /// Of dimension 1
            /// </summary>
            Scalar3Register,

            /// <summary>
            /// Of dimension 1
            /// </summary>
            Scalar4Register,

            /// <summary>
            /// Of dimension 1
            /// </summary>
            Scalar5Register,

            /// <summary>
            /// Of dimension 1
            /// </summary>
            Scalar6Register,

            /// <summary>
            /// Of dimension 1
            /// </summary>
            Scalar7Register,

            /// <summary>
            /// Of dimension 1
            /// </summary>
            Scalar8Register,

            /// <summary>
            /// Of dimension CARTESIAN_DIM
            /// </summary>
            CartVec1Register,

            /// <summary>
            /// Of dimension CARTESIAN_DIM
            /// </summary>
            CartVec2Register,

            /// <summary>
            /// Of dimension CARTESIAN_DIM
            /// </summary>
            CartVec3Register,

            /// <summary>
            /// Of dimension NUM_JOINTS
            /// </summary>
            JointVec1Register,

            /// <summary>
            /// Of dimension NUM_JOINTS
            /// </summary>
            JointVec2Register,

            /// <summary>
            /// Of dimension NUM_JOINTS
            /// </summary>
            JointVec3Register,

            /// <summary>
            /// Of dimension CARTESIAN_FRAME_LENGTH
            /// </summary>
            Frame1Register,

            /// <summary>
            /// Of dimension CARTESIAN_FRAME_LENGTH
            /// </summary>
            Frame2Register,

            /// <summary>
            /// Of dimension CARTESIAN_FRAME_LENGTH
            /// </summary>
            Frame3Register,

            /// <summary>
            /// Of dimension 1
            /// </summary>
            CurrentControllerStateNumber,

            /// <summary>
            /// Of dimension 1
            /// </summary>
            NumberOfWorkingRegisters
        }

        /// <summary> 
        /// Enumeration of feedback registers for KUKA LBR 4+ 
        /// Starting at the number of machine registers so we have a consistent way of indexing into a
        /// single array containing all registers.  These registers all correspond to registers provided in the
        /// FRI request by the FRI client.  See the FRI documentation for more information:
        /// <see href="http://sharepoint/sites/Baker/SiteAssets/Baker%20Wiki/LBR4+%20Manuals/KUKA_FRI_10_en.pdf" />
        /// </summary>
#if CSHARP
        public enum
#else
            ;
    enum KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT
#endif
 KukaLBR4PlusFeedbackRegisters
        {
            /// <summary>
            /// Of dimension 1
            /// </summary>
            TimestampInMsRegister,

            /// <summary>
            /// Of dimension 1
            /// </summary>
            FRIStateRegister,

            /// <summary>
            /// Of dimension 1
            /// </summary>
            UDPQualityRegister,

            /// <summary>
            /// Of dimension 1
            /// </summary>
            CycleTimeInMsRegister,

            /// <summary>
            /// Of dimension 1
            /// </summary>
            SafetyLimitInPercentRegister,

            /// <summary>
            /// Of dimension 1
            /// </summary>
            DrivePowerBitFieldRegister,

            /// <summary>
            /// Of dimension 1
            /// </summary>
            ControllerStrategyRegister,

            /// <summary>
            /// Of dimension NUM_JOINTS
            /// </summary>
            JointTemperatureRegister,

            /// <summary>
            /// Of dimension NUM_JOINTS
            /// </summary>
            MeasuredJointPositionRegister,

            /// <summary>
            /// Of dimension NUM_JOINTS
            /// </summary>
            MeasuredJointVelocityRegister,

            /// <summary>
            /// Of dimension NUM_JOINTS
            /// </summary>
            CommandedJointPositionRegister,

            /// <summary>
            /// Of dimension NUM_JOINTS
            /// </summary>
            CommandedJointOffsetRegister,

            /// <summary>
            /// Of dimension CARTESIAN_FRAME_LENGTH
            /// </summary>
            MeasuredCartesianPositionRegister,

            /// <summary>
            /// Of dimension CARTESIAN_FRAME_LENGTH
            /// </summary>
            CommandedCartesianPositionRegister,

            /// <summary>
            /// Of dimension CARTESIAN_FRAME_LENGTH
            /// </summary>
            CommandedCartesianOffsetRegister,

            /// <summary>
            /// Of dimension NUM_JOINTS
            /// </summary>
            MeasuredJointTorqueRegister,

            /// <summary>
            /// Of dimension NUM_JOINTS
            /// </summary>
            MeasuredExternalJointTorqueRegister,

            /// <summary>
            /// Of dimension CARTESIAN_DIM
            /// </summary>
            EstimatedTCPForceTorqueRegister,

            /// <summary>
            /// Of dimension CARTESIAN_DIM * NUM_JOINTS
            /// </summary>
            JacobianRegister,

            /// <summary>
            /// Of dimension CARTESIAN_DIM * NUM_JOINTS
            /// </summary>
            MassMatrixRegister,

            /// <summary>
            /// Of dimension NUM_JOINTS
            /// </summary>
            GravityCompensationRegister,

            /// <summary>
            /// Of dimension NUM_JOINTS
            /// </summary>
            LastIssuedJointPositionCommandRegister,

            /// <summary>
            /// Of dimension
            /// </summary>
            NumberOfFeedbackRegisters
        }

#if !CSHARP
            ;
    const unsigned int KukaLBR4PlusTotalNumberOfRegisters =
	KukaLBR4PlusWorkingRegisters::NumberOfWorkingRegisters + KukaLBR4PlusFeedbackRegisters::NumberOfFeedbackRegisters;
#endif

        /// <summary>
        /// Control modes supported by the KUKA LBR 4+.
        /// </summary>
#if CSHARP
        public
#endif
 enum ControlModes
        {
            /// <summary> Send response to FRI client but take no control action. </summary>
            NoneControlMode,

            /// <summary> Execute joint position control mode using current register values for command values. </summary>
            JointPositionControlMode,

            /// <summary> Execute joint impedance control mode using current register values for command values. </summary>
            JointImpedanceControlMode,

            /// <summary> Execute Cartesian impedance control mode using current register values for command values.  </summary>
            CartesianImpedanceControlMode,

            /// <summary> Execute Cartesian impedance control mode by passing current register values to KRL script for execution. </summary>
            KrlCartesianImpedanceControlMode
        }

        /// <summary>
        /// Enumeration of available internal controller state operations and transition tests/operations.
        /// The order that the operations are enumerated in must match the ordering of the function pointers
        /// in <c>opPointers</c> defined in the <c>ControllerThreadFunction</c> in <c>KukaLBR4PlusController.cpp</c>
        /// </summary>
#if CSHARP
        public
#else
        ;
#endif

 enum KukaLBR4Operations
        {
            /// <summary>
            /// Add two scalars.
            /// Scalar0 = Scalar1 + Scalar2
            /// </summary>
            AddOp,

            /// <summary>
            /// Add two 6 dimensional vectors. 
            /// Vector0 = Vector1 + Vector2
            /// </summary>
            AddVec6Op,

            /// <summary>
            /// Determine if any element of a 6-dim vector is greater than the corresponding element in a reference vector.
            /// </summary>
            IsAnyElementGreaterThanVec6Op,

            /// <summary>
            /// Determine if the magnitude of a 6-dim vector (input1) is greater than some reference scalar (input2).
            /// </summary>
            IsMagnitudeGreaterThanVec6Op,

            /// <summary>
            /// Multiply two scalars. 
            /// </summary>
            MultiplyOp,

            /// <summary>
            /// 6 DOF interpolation, 3 linear DOF, 3 rot DOF (Cartesian space)
            /// </summary>
            SimpleCartesianInterpolationOp,

            /// <summary>
            /// 7 DOF interpolation (KUKA LBR joint space)
            /// </summary>
            SimpleJointInterpolationOp,

            /// <summary>
            /// Compare two scalars for equality
            /// </summary>
            AreScalarsEqualOp,

            /// <summary>
            /// Adds two 7 dimensional vectors
            /// Vector0 = Vector1 + Vector2
            /// </summary>
            AddVec7Op,

            /// <summary>
            /// Copy a 7 dimensional vector to another.
            /// Vector0 - Destination
            /// Vector1 - Source
            /// </summary>
            CopyVec7Op,

            /// <summary>
            /// Subtract two 7 dimensional vectors
            /// Vector0 = Vector1 - Vector2
            /// </summary>
            SubtractVec7Op,

            /// <summary>
            /// Copy a vector of length 6
            /// </summary>
            CopyVec6Op,

            /// <summary>
            /// Copies a vector of length 12 (frame)
            /// </summary>
            CopyFrameOp,

            /// <summary>
            /// Multiples two frames represented as 12 dimensional row-major arrays.
            /// </summary>
            MultiplyFramesOp,

            /// <summary>
            /// Returns true if the magnitude of the given force is greater than the supplied scalar.
            /// </summary>
            IsForceMagnitudeGreaterThanScalarOp,

            /// <summary>
            /// Returns true if magnitude of the given force projected into the given plane (defined by a vector
            /// orthogonal to the plane) is greater than the provided threshold.
            /// </summary>
            IsForceInPlaneGreatherThanScalarOp,

            /// <summary>
            /// Returns true if magnitude of the given force rejected from the given plane (defined by a vector
            /// orthogonal to the plane) is greater than the provided threshold.
            /// </summary>
            IsForceOrthogonalToPlaneGreaterThanScalarOp,

            /// <summary>
            /// Moves the LBR to the candle position at a constant joint velocity (same for all joints)
            /// </summary>
            GetJointAnglesForMotionToCandleAtConstantRate
        }

#if CSHARP
    }
}
#else
;
#endif

#endif
